#include "main_loop.h"
#include "as5600.h"
#include "calculation.h"
#include "motor.h"
const float powor_out_max = 10;

float electrical_angle(float shaft_angle, int pole_paire)
{
    return shaft_angle * pole_paire;
}

float normalize_angle(float angle)
{
    float a = fmod(angle, _2PI);
    return a >= 0 ? a : (a + _2PI);
}

void set_PWM_out(float ua, float ub, float uc)
{
    float da, db, dc;
    da = ua / powor_out_max;
    db = ub / powor_out_max;
    dc = uc / powor_out_max;
    PWM_OUT(1, da * 1440);
    PWM_OUT(2, db * 1440);
    PWM_OUT(3, dc * 1440);
}

// 帕克变换 克拉克变换
void set_phase_voltage(float uq, float ud, float angle_el)
{
    float ua, ub, uc;
    float Ualpha, Ubeta;
    // 帕克变换
    angle_el = normalize_angle(angle_el);
    Ualpha = uq * _sin(angle_el);
    Ubeta = uq * _cos(angle_el);
    // 克拉克变换
    ua = Ualpha + powor_out_max / 2;
    ub = (_sqrtApprox(3) * Ubeta - Ualpha) / 2 + powor_out_max / 2;
    uc = (-_sqrtApprox(3) * Ubeta - Ualpha) / 2 + powor_out_max / 2;
    set_PWM_out(ua, ub, uc);
}
float velocity_open_loop(float target_v)
{
    float angle;
    angle = getAngle();
    angle = normalize_angle((angle + (target_v * 0.01)));
    set_phase_voltage(powor_out_max, 0, electrical_angle(angle, 7));
    return powor_out_max;
}
